/*****************************************************************
**                大连理工大学 凌BUG战队
**                **生死有命，富贵在天**
**    没有什么难题是一个通宵解决不了的，如果有那就整两个！
**---------------------------------------------------------------
** 项目名称：   mxdemo_std_robot
** 日    期：   2021-04-15
** 作    者：   MasterWang
**---------------------------------------------------------------
** 文 件 名：   bmi088.c
** 文件说明：   三轴陀螺仪和三轴加速度计BMI088芯片数据采集
*****************************************************************/

/*---------------------INCLUDES----------------------*/
#include "bmi088.h"
#include "spi.h"
#include "flash.h"
#include "music.h"

/*---------------------VARIABLES---------------------*/
//使用CCM数据RAM区(0x10000000-0x10000FFF 4KB)
//
uint8_t gyro_zero_calib_flag __attribute__((at(0x10000000))) = 0; //进入陀螺仪标定程序的标志位在姿态解算里很重要尤其是飞行器

//BMI088采集数据
volatile float bmi088_gyro[3] __attribute__((at(0x10000010))) = {0.0f};
volatile float bmi088_gyro_deg[3]__attribute__((at(0x10000020))) = {0.0f};
float bmi088_accel[3] __attribute__((at(0x10000030))) = {0.0f};
float bmi088_accel_last[3] __attribute__((at(0x10000040))) = {0.0f};
float bmi088_temperature __attribute__((at(0x10000050))) = 0.0f;

//陀螺仪零漂常数
float bmi088_gyro_zero[3] __attribute__((at(0x10000060))) = {0.0f};
float bmi088_gyro_deg_zero[3] __attribute__((at(0x10000070))) = {0.0f};

//校准后陀螺仪数据
float bmi088_gyro_calib[3] __attribute__((at(0x10000080))) = {0.0f};
float bmi088_gyro_deg_calib[3] __attribute__((at(0x10000090))) = {0.0f};

/*---------------------FUNCTIONS---------------------*/

/***********************************************************************
** 函 数 名： BMI088_read_write_byte()
** 函数说明： 和BMI088交换一字节数据
**---------------------------------------------------------------------
** 输入参数： 发送字节
** 返回参数： 接收字节
***********************************************************************/
uint8_t BMI088_read_write_byte(uint8_t tx_data)
{
    uint8_t rx_data;
    HAL_SPI_TransmitReceive(&hspi1, &tx_data, &rx_data, 1, 1000);
    return rx_data;
}

/***********************************************************************
** 函 数 名： BMI088_ACCEL_NS_L()
** 函数说明： BMI088芯片加速度使能引脚拉低(有效)
**---------------------------------------------------------------------
** 输入参数： 无
** 返回参数： 无
***********************************************************************/
void BMI088_ACCEL_NS_L(void)
{
    HAL_GPIO_WritePin(BMI088_ACCEL_CS1_GPIO_Port, BMI088_ACCEL_CS1_Pin, GPIO_PIN_RESET);
}

/***********************************************************************
** 函 数 名： BMI088_ACCEL_NS_H()
** 函数说明： BMI088芯片加速度使能引脚拉高
**---------------------------------------------------------------------
** 输入参数： 无
** 返回参数： 无
***********************************************************************/
void BMI088_ACCEL_NS_H(void)
{
    HAL_GPIO_WritePin(BMI088_ACCEL_CS1_GPIO_Port, BMI088_ACCEL_CS1_Pin, GPIO_PIN_SET);
}

/***********************************************************************
** 函 数 名： BMI088_GYRO_NS_L()
** 函数说明： BMI088芯片陀螺仪使能引脚拉低(有效)
**---------------------------------------------------------------------
** 输入参数： 无
** 返回参数： 无
***********************************************************************/
void BMI088_GYRO_NS_L(void)
{
    HAL_GPIO_WritePin(BMI088_GYRO_CS1_GPIO_Port, BMI088_GYRO_CS1_Pin, GPIO_PIN_RESET);
}

/***********************************************************************
** 函 数 名： BMI088_GYRO_NS_H()
** 函数说明： BMI088芯片陀螺仪使能引脚拉高
**---------------------------------------------------------------------
** 输入参数： 无
** 返回参数： 无
***********************************************************************/
void BMI088_GYRO_NS_H(void)
{
    HAL_GPIO_WritePin(BMI088_GYRO_CS1_GPIO_Port, BMI088_GYRO_CS1_Pin, GPIO_PIN_SET);
}

/***********************************************************************
** 函 数 名： BMI088_accel_write_single_reg()
** 函数说明： 写入BMI088加速度一个寄存器
**---------------------------------------------------------------------
** 输入参数： 寄存器地址,写入数据
** 返回参数： 无
***********************************************************************/
void BMI088_accel_write_single_reg(uint8_t reg, uint8_t data)
{
    BMI088_ACCEL_NS_L();
    BMI088_read_write_byte(reg);
    BMI088_read_write_byte(data);
    BMI088_ACCEL_NS_H();
}

/***********************************************************************
** 函 数 名： BMI088_accel_read_single_reg()
** 函数说明： 读取BMI088加速度一个寄存器
**---------------------------------------------------------------------
** 输入参数： 寄存器地址
** 返回参数： 读取数据
***********************************************************************/
uint8_t BMI088_accel_read_single_reg(uint8_t reg)
{
    uint8_t tmp;

    BMI088_ACCEL_NS_L();
    BMI088_read_write_byte(reg | 0x80);
    BMI088_read_write_byte(0x55);
    tmp = BMI088_read_write_byte(0x55);
    BMI088_ACCEL_NS_H();

    return tmp;
}

/***********************************************************************
** 函 数 名： BMI088_accel_read_muli_reg()
** 函数说明： 读取BMI088加速度多个寄存器
**---------------------------------------------------------------------
** 输入参数： 起始寄存器地址,读取数据缓冲区,读取个数
** 返回参数： 无
***********************************************************************/
void BMI088_accel_read_muli_reg(uint8_t reg, uint8_t* buf, uint8_t num)
{
    BMI088_ACCEL_NS_L();
    BMI088_read_write_byte(reg | 0x80);
    BMI088_read_write_byte(0x55);
    while(num != 0)
    {
        *buf = BMI088_read_write_byte(0x55);
        buf++;
        num--;
    }
    BMI088_ACCEL_NS_H();
}

/***********************************************************************
** 函 数 名： BMI088_gyro_write_single_reg()
** 函数说明： 写入BMI088陀螺仪一个寄存器
**---------------------------------------------------------------------
** 输入参数： 寄存器地址,写入数据
** 返回参数： 无
***********************************************************************/
void BMI088_gyro_write_single_reg(uint8_t reg, uint8_t data)
{
    BMI088_GYRO_NS_L();
    BMI088_read_write_byte(reg);
    BMI088_read_write_byte(data);
    BMI088_GYRO_NS_H();
}

/***********************************************************************
** 函 数 名： BMI088_gyro_read_single_reg()
** 函数说明： 读取BMI088陀螺仪一个寄存器
**---------------------------------------------------------------------
** 输入参数： 寄存器地址
** 返回参数： 读取数据
***********************************************************************/
uint8_t BMI088_gyro_read_single_reg(uint8_t reg)
{
    uint8_t tmp;

    BMI088_GYRO_NS_L();
    BMI088_read_write_byte(reg | 0x80);
    tmp = BMI088_read_write_byte(0x55);
    BMI088_GYRO_NS_H();

    return tmp;
}

/***********************************************************************
** 函 数 名： BMI088_gyro_read_muli_reg()
** 函数说明： 读取BMI088陀螺仪多个寄存器
**---------------------------------------------------------------------
** 输入参数： 起始寄存器地址,读取数据缓冲区,读取个数
** 返回参数： 无
***********************************************************************/
void BMI088_gyro_read_muli_reg(uint8_t reg, uint8_t* buf, uint8_t num)
{
    BMI088_GYRO_NS_L();
    BMI088_read_write_byte(reg | 0x80);
    while(num != 0)
    {
        *buf = BMI088_read_write_byte(0x55);
        buf++;
        num--;
    }
    BMI088_GYRO_NS_H();
}

/***********************************************************************
** 函 数 名： BMI088_init()
** 函数说明： 初始化BMI088寄存器配置(加速度 量程±24g 频率800Hz / 陀螺仪 量程±2000°/s 频率1000Hz)
**---------------------------------------------------------------------
** 输入参数： 无
** 返回参数： 无
***********************************************************************/
void BMI088_init(void)
{
    uint8_t res = 0;

    //
    BMI088_accel_write_single_reg(0x7E, 0xB6); //soft_rest
    HAL_Delay(100);
    res = BMI088_accel_read_single_reg(0x00);
    HAL_Delay(150);
    res = BMI088_accel_read_single_reg(0x00);
    HAL_Delay(150);
    if(res != 0x1E)
    {
        SetBuzzerState(PLAYING_ERROR_SOUND);   //chip id 0x1E
    }

    BMI088_accel_write_single_reg(0x40, 0xAB);
    HAL_Delay(10); //ACC_CONF 1010 1010 800Hz,230/200Hz
    BMI088_accel_write_single_reg(0x41, 0x03);
    HAL_Delay(10); //ACC_RANGE 00-3g;01-6g;02-12g;03-24g
    BMI088_accel_write_single_reg(0x53, 0x08);
    HAL_Delay(10); //INT1_IO_CONF 0000 1000 INT1 as output pp acitiv_low
    BMI088_accel_write_single_reg(0x58, 0x04);
    HAL_Delay(10); //INT_MAP_DATA 0000 0100 INT1 drdy interrupt
    BMI088_accel_write_single_reg(0x7C, 0x00);
    HAL_Delay(50); //power config-acitve mode
    BMI088_accel_write_single_reg(0x7D, 0x04);
    HAL_Delay(50); //power control-acc enable

    //
    BMI088_gyro_write_single_reg(0x14, 0xB6); //soft_rest
    HAL_Delay(100);
    res = BMI088_gyro_read_single_reg(0x00);
    HAL_Delay(150);
    if(res != 0x0F)
    {
        SetBuzzerState(PLAYING_ERROR_SOUND);   //chip id 0x0F
    }

    BMI088_gyro_write_single_reg(0x0F, 0x00);
    HAL_Delay(10); //GYRO_RANGE 00-2000°/s;01-1000°/s;02-500°/s;03-250°/s;04-125°/s
    BMI088_gyro_write_single_reg(0x10, 0x82);
    HAL_Delay(10); //GYRO_ODR,BANDWIDTH 00-2000Hz,532Hz;01-2000Hz,230Hz;02-1000Hz,116Hz;03-400Hz,47Hz;04-200Hz,23Hz;05-100Hz,12Hz;06-200Hz,64Hz;07-100Hz,32Hz
    BMI088_gyro_write_single_reg(0x11, 0x00);
    HAL_Delay(10); //GYRO_LPM1 normal mode
    BMI088_gyro_write_single_reg(0x15, 0x80);
    HAL_Delay(10); //GYRO_INT_CTRL 1000 0000 enable drdy
    BMI088_gyro_write_single_reg(0x16, 0x00);
    HAL_Delay(10); //INT_IO_CONF 0000 0000 pp active_low
    BMI088_gyro_write_single_reg(0x18, 0x01);
    HAL_Delay(10); //INT_IO_MAP 0000 0001 drdy to INT3

    //read flash
    flash_read_address(GYRO_FLASH_ADDRESS + 0, (uint32_t*)bmi088_gyro_zero, 3);
    flash_read_address(GYRO_FLASH_ADDRESS + 12, (uint32_t*)bmi088_gyro_deg_zero, 3);
}

/***********************************************************************
** 函 数 名： BMI088_calib_gyro_zero()
** 函数说明： 标定BMI088三轴陀螺仪的零漂值
**---------------------------------------------------------------------
** 输入参数： 三轴陀螺仪弧度值,三轴陀螺仪角度值
** 返回参数： 无
***********************************************************************/
void BMI088_calib_gyro_zero(volatile float gyro[], volatile float gyro_deg[])
{
    uint16_t n;
    float sum[3] = {0.0f, 0.0f, 0.0f};
    float sum_deg[3] = {0.0f, 0.0f, 0.0f};

    for(n = 0; n < 1000; n++)
    {
        sum[0] += gyro[0];
        sum[1] += gyro[1];
        sum[2] += gyro[2];

        sum_deg[0] += gyro_deg[0];
        sum_deg[1] += gyro_deg[1];
        sum_deg[2] += gyro_deg[2];

        HAL_Delay(10);
    }

    //
    bmi088_gyro_zero[0] = sum[0] / 1000.0f;
    bmi088_gyro_zero[1] = sum[1] / 1000.0f;
    bmi088_gyro_zero[2] = sum[2] / 1000.0f;

    bmi088_gyro_deg_zero[0] = sum_deg[0] / 1000.0f;
    bmi088_gyro_deg_zero[1] = sum_deg[1] / 1000.0f;
    bmi088_gyro_deg_zero[2] = sum_deg[2] / 1000.0f;

    //write flash
    flash_erase_address(GYRO_FLASH_ADDRESS, 1);
    flash_write_address(GYRO_FLASH_ADDRESS + 0, (uint32_t*)bmi088_gyro_zero, 3);
    flash_write_address(GYRO_FLASH_ADDRESS + 12, (uint32_t*)bmi088_gyro_deg_zero, 3);
}

/***********************************************************************
** 函 数 名： BMI088_read_gyro()
** 函数说明： 读取BMI088三轴陀螺仪值
**---------------------------------------------------------------------
** 输入参数： 无
** 返回参数： 无
***********************************************************************/
void BMI088_read_gyro(void)
{
    uint8_t buf[6];
    int16_t bmi088_raw_tmp;

    //
    BMI088_gyro_read_muli_reg(0x02, buf, 6);

    bmi088_raw_tmp = (int16_t)((buf[1] << 8) | buf[0]);
    bmi088_gyro[0] = bmi088_raw_tmp * 0.00106526443603169529841533860381f; //((tmp/32768)*(2000*pi/180))rad/s
    bmi088_gyro_deg[0] = bmi088_raw_tmp * 0.06103515625f; //((tmp/32768)*2000)deg/s

    bmi088_raw_tmp = (int16_t)((buf[3] << 8) | buf[2]);
    bmi088_gyro[1] = bmi088_raw_tmp * 0.00106526443603169529841533860381f;
    bmi088_gyro_deg[1] = bmi088_raw_tmp * 0.06103515625f;

    bmi088_raw_tmp = (int16_t)((buf[5] << 8) | buf[4]);
    bmi088_gyro[2] = bmi088_raw_tmp * 0.00106526443603169529841533860381f;
    bmi088_gyro_deg[2] = bmi088_raw_tmp * 0.06103515625f;

    //减去标定的零漂值
    bmi088_gyro_calib[0] = bmi088_gyro[0] - bmi088_gyro_zero[0];
    bmi088_gyro_calib[1] = bmi088_gyro[1] - bmi088_gyro_zero[1];
    bmi088_gyro_calib[2] = bmi088_gyro[2] - bmi088_gyro_zero[2];

    bmi088_gyro_deg_calib[0] = bmi088_gyro_deg[0] - bmi088_gyro_deg_zero[0];
    bmi088_gyro_deg_calib[1] = bmi088_gyro_deg[1] - bmi088_gyro_deg_zero[1];
    bmi088_gyro_deg_calib[2] = bmi088_gyro_deg[2] - bmi088_gyro_deg_zero[2];
}

/***********************************************************************
** 函 数 名： BMI088_read_accel()
** 函数说明： 读取BMI088三轴加速度值
**---------------------------------------------------------------------
** 输入参数： 无
** 返回参数： 无
***********************************************************************/
void BMI088_read_accel(void)
{
    uint8_t buf[6];
    int16_t bmi088_raw_tmp;

    //
    BMI088_accel_read_muli_reg(0x12, buf, 6);

    bmi088_raw_tmp = (int16_t)((buf[1] << 8) | buf[0]);
    bmi088_accel[0] = bmi088_raw_tmp * 0.00718260498046875f; //(tmp/32768)*24*9.80665 m/s2
    bmi088_raw_tmp = (int16_t)((buf[3] << 8) | buf[2]);
    bmi088_accel[1] = bmi088_raw_tmp * 0.00718260498046875f;
    bmi088_raw_tmp = (int16_t)((buf[5] << 8) | buf[4]);
    bmi088_accel[2] = bmi088_raw_tmp * 0.00718260498046875f;

    //一阶低通滤波
    bmi088_accel[0] = bmi088_accel[0] * 0.3f + bmi088_accel_last[0] * 0.7f;
    bmi088_accel[1] = bmi088_accel[1] * 0.3f + bmi088_accel_last[1] * 0.7f;
    bmi088_accel[2] = bmi088_accel[2] * 0.3f + bmi088_accel_last[2] * 0.7f;

    bmi088_accel_last[0] = bmi088_accel[0];
    bmi088_accel_last[1] = bmi088_accel[1];
    bmi088_accel_last[2] = bmi088_accel[2];
}

/***********************************************************************
** 函 数 名： BMI088_read_temperature()
** 函数说明： 读取BMI088芯片温度
**---------------------------------------------------------------------
** 输入参数： 无
** 返回参数： 无
***********************************************************************/
void BMI088_read_temperature(void)
{
    uint8_t buf[2];
    int16_t bmi088_raw_tmp;

    //
    BMI088_accel_read_muli_reg(0x22, buf, 2);

    bmi088_raw_tmp = (int16_t)((buf[0] << 3) | (buf[1] >> 5));
    if(bmi088_raw_tmp > 1023)
    {
        bmi088_raw_tmp -= 2048;
    }
    bmi088_temperature = bmi088_raw_tmp * 0.125f + 23.0f;
}
